#include "stanley_control/stanley_controller.h"

namespace stanley_control{

    void StanleyController::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){

        // yaw =tf::getYaw(msg->pose.pose.orientation);
        // tf::pointMsgToTF(msg->pose.pose.position,current_pose);
    }

    void StanleyController::OdomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) {
    
        yaw =tf::getYaw(odom_msg->pose.pose.orientation);
        tf::pointMsgToTF(odom_msg->pose.pose.position,current_pose);
    }

    auto StanleyController::distance(path_type p1,path_type p2) -> double{
        return sqrt(pow(p1.first.first-p2.first.first,2) +
                    pow(p1.first.second-p2.first.second,2));
    }
    auto StanleyController::GetMinDisIndex(path_type current_pose, std::vector<path_type> path) -> int{
        double min_dis = INFINITY;
        int idx = 0;
        for(int i = 0;i<path.size();i++){
            double dis  = distance(path[i],current_pose);
            if(dis < min_dis){
                min_dis = dis;
                idx = i;
            }
        }
        return idx;
    }

    void StanleyController::run(){
        ROS_INFO("Stanley-control algorithm is running!");
        cmd_vel_pub = n_.advertise<geometry_msgs::Twist>("/cmd_vel",1);
        pose_sub = n_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",10,
                            std::bind(&StanleyController::robotPoseCallBack,this,std::placeholders::_1));
        path_sub = n_.subscribe<nav_msgs::Path>("/rc_path_pub",10,std::bind(&StanleyController::robotPathCallBack,this,std::placeholders::_1));
        real_path_pub = n_.advertise<nav_msgs::Path>("rvizpath", 100, true);

        odom_pose_sub = n_.subscribe<nav_msgs::Odometry>("odom", 10, 
            std::bind(&StanleyController::OdomCallback, this, std::placeholders::_1));

        ros::Rate loop_rate(10);

        recv_path_flag = true;

        path.header.frame_id = "/map";
        // 设置时间戳
        path.header.stamp = ros::Time::now();

        path_type init_pose = std::make_pair(std::make_pair(0.0,0.0),0.0);
        path_type now_pose = init_pose;

        while(ros::ok()){

            now_pose.first.first = current_pose.x();
            now_pose.first.second = current_pose.y();
            now_pose.second = yaw;
            ROS_INFO_STREAM(
                "now_pose.first.first:"<<now_pose.first.first
                <<"now_pose.first.second:"<<now_pose.first.second
                <<"now_pose.second:"<<now_pose.second);
            
            if(path_vector.size()>0){
                int current_idx = GetMinDisIndex(now_pose,path_vector);
                path_type cur_path_point = path_vector[current_idx];

                //横向偏差
                double e_y = distance(now_pose,cur_path_point);
                e_y = (now_pose.first.second - cur_path_point.first.second) * cos(cur_path_point.second) - 
                        (now_pose.first.first - cur_path_point.first.first) * sin(cur_path_point.second) <=0 ? e_y : -e_y;
                ROS_INFO_STREAM("e_y:" << e_y);
                //航向偏差,转到-PI 到 +PI之间
                //存疑，为什么不是自车航向角-参考点航向角？？
                double theta_e = -now_pose.second + cur_path_point.second;  //参考点航向 - 自车航向角
                if(theta_e > M_PI){ 
                    theta_e = theta_e - 2* M_PI;
                } else if(theta_e < -M_PI){
                    theta_e = theta_e + 2* M_PI;
                }
                ROS_INFO_STREAM("theta_e:" << theta_e);

                //转角，限幅
                double delta =( theta_e + atan2(factor * e_y, speed) );
                // if(delta > M_PI/3){
                //     delta = M_PI/3;
                // } else if(delta < -M_PI/3){
                //     delta = -M_PI/3;
                // }


                ROS_INFO_STREAM("delta:" << delta);

                //直接把转角当作角速度
                vel.linear.x = speed;
                vel.angular.z = delta * 2.5;
                cmd_vel_pub.publish(vel);

                //发布实际轨迹
                geometry_msgs::PoseStamped this_pose_stamped;
                this_pose_stamped.pose.position.x = current_pose.x();
                this_pose_stamped.pose.position.y = current_pose.y();
                this_pose_stamped.pose.position.z = 0.0;
                this_pose_stamped.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
                this_pose_stamped.header.stamp = ros::Time::now();
                this_pose_stamped.header.frame_id = "/map";
                path.poses.push_back(this_pose_stamped);


            }
                real_path_pub.publish(path);

            ros::spinOnce();
            loop_rate.sleep();
        }
    }

    void StanleyController::robotPathCallBack(const nav_msgs::PathConstPtr& msg){
        if(recv_path_flag){
            nav_path_.poses = msg->poses;
            recv_path_flag = false;
            start_pose_.position.x = nav_path_.poses[0].pose.position.x;
            start_pose_.position.y = nav_path_.poses[0].pose.position.y;

            

            path_vector.clear();
            for(int i = 0;i<nav_path_.poses.size();i++){
                path_type temp_path = std::make_pair(std::make_pair(nav_path_.poses[i].pose.position.x,nav_path_.poses[i].pose.position.y),
                                                    tf::getYaw(nav_path_.poses[i].pose.orientation));
                std::cout<<"temp_path.first.first = "<<temp_path.first.first<<std::endl;
                std::cout<<"temp_path.first.second = "<<temp_path.first.second<<std::endl;
                std::cout<<"temp_path.second = "<<temp_path.second<<std::endl;
                std::cout<<"------------------------------------------------------------------------"<<std::endl;
                path_vector.push_back(temp_path);
            }
            std::cout<<"path_vector.size() = "<<path_vector.size()<<std::endl;
        }

    }

}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"stanley_controller");
    ros::NodeHandle n;
    auto node = new stanley_control::StanleyController(n);
    node->run();
    return 0;
}
